#ifndef __MY_UTILS_H__
#define __MY_UTILS_H__

#include <stdint.h>
#include <stdio.h>
#include <math.h>

class TestUtils
{
private:
    int count;

public:
    TestUtils();
    ~TestUtils();
    int get_count();
    void add_count();
};

TestUtils::TestUtils()
{
    this->count = 0;
}

TestUtils::~TestUtils()
{
}

/**
 * @brief  S型速度曲线规划
 */
class SpeedUtils
{
private:
    int speed_max_; /// 最大速度，mm/s
    int acc_max_;   /// 最大加速度，mm/s^2
    int jerk_;  /// 加加速度，每秒加速度增量， mm/s^3
    double delta;

    double overall;  /// 路径总长度，mm
    int cur_speed;  /// 当前条件下的速度
    int cur_acc_max;   /// 当前条件下的最大加速度
    int cur_acc;

    double choice[2];
    double time[8];
    double len[8];

    double len_sum(int start, int end);
public:
    SpeedUtils(int speed_max, int acc_max, int jerk);
    ~SpeedUtils();
    void setGoal(double overall);
    int getSpeed(int walked);
};

SpeedUtils::SpeedUtils(int speed_max, int acc_max, int jerk = 100)
{
    this->speed_max_ = speed_max;
    this->acc_max_ = acc_max;
    this->jerk_ = jerk;
    this->cur_speed = 0;
    this->cur_acc = 0;
    this->delta = 0.1;

    double t1 = acc_max / jerk;
    double t2 = (speed_max - t1 * acc_max) / acc_max;
    this->choice[0] = jerk * pow(t1, 3);
    this->choice[1] = 0.5 * acc_max * pow(t1, 2) + 0.5 * acc_max * pow(t2, 2);
}

SpeedUtils::~SpeedUtils()
{
}

#endif